﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace MotionPlanning.MotionPlanningComponents.MotionPlanningSystems
{
    class System_TwoLink : RoboticSystemComponent
    {
        private double m1;
        private double m2;
        private double L1;
        private double L2;
        private double J1;
        private double J2;
        private double K11;
        private double K12;
        private double K21;
        private double K22;
        private double K31;
        private double K32;
        private double g;

        override public string ToString()
        {
            return "\n\t<System_Type>Robotics_TwoLink</System_Type>" +
                    base.ToString();
        }

        override public double Distance2_Topological(double[] x1, double[] x2)
        {

            double diff_q3 = (x1[2] - x2[2]) * q_scale[2];
            double diff_q4 = (x1[3] - x2[3]) * q_scale[3];

            double diff_q1 = Math.Abs(x1[0] - x2[0]);
            while (diff_q1 > MotionPlanningUtils.PI_double)
                diff_q1 -= MotionPlanningUtils.PI_double;
            if (diff_q1 > Math.PI)
                diff_q1 = MotionPlanningUtils.PI_double - diff_q1;
            diff_q1 *= q_scale[0];

            double diff_q2 = Math.Abs(x1[1] - x2[1]);
            while (diff_q2 > MotionPlanningUtils.PI_double)
                diff_q2 -= MotionPlanningUtils.PI_double;
            if (diff_q2 > Math.PI)
                diff_q2 = MotionPlanningUtils.PI_double - diff_q2;
            diff_q2 *= q_scale[1];

            return diff_q1 * diff_q1 + diff_q2 * diff_q2 + diff_q3 * diff_q3 + diff_q4 * diff_q4;
        }

        override public bool LoadingSystemParameters(string fileName)
        {
            base.LoadingSystemParameters(fileName);

            g = 9.8;
            m1 = 2;
            m2 = 4;
            L1 = 1;
            L2 = 1;
            J1 = m1 * L1 * L1 / 12.0;
            J2 = m2 * L2 * L2 / 12.0;
            K11 = m2 * L1 * L2 * 0.5;
            K12 = m2 * L1 * L2 * 0.5;
            K21 = m1 * g * L1 * 0.5 + m2 * g * L1;
            K22 = m2 * g * L2 * 0.5;
            K31 = m2 * L1 * L2 * 0.5;
            K32 = m2 * L1 * L2 * 0.5;

            return true;
        }

        override public void Qd(double[] q, double[] ctrl, ref double[] qd)
        {
            double sinq2q1 = Math.Sin(q[1] - q[0]);
            double cosq2q1 = Math.Cos(q[1] - q[0]);
            double cosq1 = Math.Cos(q[0]);
            double cosq2 = Math.Cos(q[1]);
            double M11 = m1 * L1 * L1 * 0.25 + m2 * L1 * L1 + J1;
            double M12 = m2 * L1 * L2 * cosq2q1 * 0.5;
            double M21 = m2 * L1 * L2 * cosq2q1 * 0.5;
            double M22 = m2 * L2 * L2 * 0.25 + J2;
            double B1 = (K11 * sinq2q1 * (q[3] - q[2]) * q[3] - K21 * cosq1 + K31 * sinq2q1 * q[2] * q[3]) + ctrl[0];
            double B2 = (K12 * sinq2q1 * (q[3] - q[2]) * q[2] - K22 * cosq2 - K32 * sinq2q1 * q[2] * q[3]) + ctrl[1];

            double Determinant = M11 * M22 - M12 * M21;
            if (Determinant == 0)
            {
                return;
            }

            qd[0] = q[2];
            qd[1] = q[3];
            qd[2] = (B1 * M22 - M12 * B2) / Determinant;
            qd[3] = (M11 * B2 - B1 * M21) / Determinant;
        }

        override public bool Steering(double[] q_start, double[] q_end, ref double[] u, ref double duration, ref double edgeCost)
        {
            throw new NotImplementedException();
        }

    }
}
